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Abstract 


This  report  introduces  a  new  terrain  map  technique  for  representing  unstructured 
environments.  Using  an  inexpensive  nodding  2-D  laser  rangefinder  this  technique 
captures  multiple  terrain  data  sets  and  optimally  fuses  the  data  into  a  wrappable  terrain 
map.  Traditional  terrain  map  implementations  scan  a  terrain  patch  only  once,  thus 
integrate  a  single  data  point  into  the  map.  The  nodding  laser,  allowing  multiple  scans 
of  a  given  terrain  patch,  must  fuse  multiple  data  sets  into  the  terrain  map.  A  moving 
vehicle,  with  a  limited  pose  accuracy,  demands  a  data  fusion  technique  that  minimizes 
the  effects  of  pose  errors.  This  research  investigated  and  tested  a  variance  weighted 
statistical  technique  to  optimally  fuse  terrain  data  sets  into  a  terrain  map.  Simulations 
and  experiments  were  conducted,  which  demonstrated  the  performance  of  the  variance 
weighted  technique  as  superior  to  classical  statistical  methods.  DRDC  implemented  a 
variance  weighted  terrain  map  under  the  Miro  architecture  and  tested  its  performance 
during  the  Autonomous  Land  Systems  (ALS)  demonstration.  This  terrain  map 
technique  performed  admirably  and  its  accurate  terrain  representation  allowed  the 
Raptor  Unmanned  Ground  Vehicle  to  successfully  navigate  while  avoiding  obstacles 
and  hazards. 

Resume 


Ce  rapport  presente  une  nouvelle  technique  de  cartes  morphographiques  visant  a 
representer  les  milieux  non  structures.  Cette  technique  peu  couteuse  qui  utilise  un 
telemetre  laser  bidimensionnel  de  balayage  en  site  capture  des  ensembles  multiples  de 
donnees  de  terrain  et  fiisionne  les  donnees  de  maniere  optimale  en  une  carte 
moiphographique  avec  bouclage  automatique.  Les  implementations  classiques  de 
cartes  morphographiques  ne  balaient  une  parcelle  de  terrain  qu’une  seule  fois  et 
n’integre  un  qu’un  seul  point  de  donnee  dans  la  carte.  Le  laser  de  balayage  en  site 
permet  d’effectuer  des  balayages  multiples  d’une  parcelle  donnee  de  terrain  et  doit 
fusionner  des  ensembles  multiples  de  donnees  dans  la  carte  morphographique.  Un 
vehicule  en  mouvement,  ayant  une  exactitude  de  pose  limitee,  exige  une  technique  de 
fusions  des  donnees  qui  minimise  les  effets  des  erreurs  de  poses.  Cette  recherche  a 
etudie  et  teste  une  technique  statistique  de  variance  ponderee  visant  a  fusionner  de 
maniere  optimale  les  ensembles  de  donnees  d’un  terrain  dans  une  carte 
moiphographique.  On  a  conduit  des  simulations  et  des  experiences  demontrant  que  le 
rendement  de  la  technique  de  variance  ponderee  est  superieur  a  celui  des  methodes 
statistiques  classiques.  RDDC  a  implemente  une  carte  moiphographique  de  variance 
ponderee  sous  une  architecture  d’echange  multimedia  d'objets  extraits  (MIRO)  et  a 
teste  son  rendement  durant  les  demonstrations  des  Systemes  de  vehicules  terrestres 
autonomes.  Cette  technique  des  cartes  moiphographiques  a  admirablement  fait  ses 
preuves  et  la  representation  exacte  du  terrain  a  permis  au  Vehicule  terrestre  sans  pilote 
Raptor,  de  reussir  a  naviguer  tout  en  evitant  des  obstacles  et  des  dangers. 
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Executive  summary 


Background:  Unmanned  Ground  Vehicle  (UGV)  Research  and  Development  within 
the  Autonomous  Land  Systems  (ALS)  project  will  assist  the  Canadian  Forces  (CF)  in 
fulfdling  their  future  mandate.  DRDC’s  Technology  Investment  Strategy  [1]  defines  an 
Autonomous  Intelligent  Systems  as  “...automated  or  robotic  systems  that  operate  and 
interact  in  the  complex  unstructured  environments  of  the  future  battlespace”.  An 
accurate  world  representation  is  a  fundamental  autonomous  intelligence  requirements. 

An  autonomous  vehicle  (UxV)  creates  a  world  representation,  analyzes  it,  and  denotes 
obstacle  locations.  Thus,  ths  Uxv  can  avoid  hazards  and  maneuver  through  obstruction 
free  terrain.  The  most  common  world  representation,  the  terrain  map,  traditionally  uses 
“single  sample”  perception  devices,  or  “stop  and  scan”  implementations.  DRDC’s 
continuous  motion  UxV  requires  a  terrain  map  technique  that  can  fuse  multiple  terrain 
samples,  acquired  under  different  poses,  into  a  single  coherent  map. 

Principle  Conclusions:  DRDC  researched,  proposed  and  implemented  a  new  and 
novel  terrain  map  data  fusion  technique.  This  variance  weighted  statistical  technique 
optimally  minimizes  pose  error  effects,  thus  creating  a  coherent  terrain  map  from 
multiple  terrain  scans.  The  variance  weighted  statistical  technique’s  performance  was 
compared  to  a  classical  statistical  technique,  and  was  determined  to  be  superior. 

Significance  of  Results:  DRDC  implemented  a  variance  weighted  terrain  map  under 
the  MIRO  framework.  Testing  on  the  Raptor  UGV  revealed  the  map’s  efficient 
implementation  consumed  relatively  few  computational  resources.  A  demonstration, 
conducted  in  the  fall  of  2005,  exercised  the  map’s  capabilities.  During  these  trials, 
DRDC’s  terrain  map  successfully  and  adequately  represented  the  world.  The  map 
remained  egocentric  while  the  Raptor  performed  translational  and  rotational 
maneuvers.  The  terrain  map  played  a  critical  role  allowing  the  Raptor  UGV  to 
successful  avoid  obstacles  and  hazards  while  autonomously  navigating  toward  a  goal. 

Future  Results:  Although  the  current  map  implementation  supported  3-D  data  from 
both  laser  ranging  devices  and  stereo  vision  cameras,  the  ALS  demonstration  relied 
exclusively  on  laser  data.  DRDC’s  stereo  vision  implementation  was  not  sufficiently 
mature  and  often  produced  disparity  maps  that  suffered  from  fringing  effects.  Without 
the  high  3-D  data  densities  produced  by  stereo  vision,  the  terrain  map  sometimes 
suffered  from  data  starvation.  Future  work  will  investigate  means  to  minimize  the 
impact  of  fringing  effects.  The  current  terrain  map,  implementing  a  2^D  world 
representation,  can  not  represent  obstacle  and  hazards  with  3  dimensional  properties. 
Obstacles  such  as  overhangs,  tunnels  and  passageways  have  height  restrictions  that  can 
preclude  safe  navigation.  Future  research  will  investigate  multiple  map 
implementations  that  should  allow  the  representation  of  such  3-D  obstacles. 


G.S.  Broten,  J.L.  Giesbrecht,  S.P.  Monckton.  2005.  World  Representation  Using  Terrain  Maps. 
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Sommaire 


Contexte  :  La  recherche  et  developpement  sur  les  Vehicules  terrestres  autonomes  au  sein  du 
projet  des  Systemes  de  vehicules  terrestres  autonomes  aideront  les  Forces  canadiennes  (CF)  a 
executer  leur  mandat  fiatur.  La  Strategic  d’investissements  technologiques  [1]  defmit  un 
Systeme  intelligent  autonome  comme  un  «  systeme  automatise  ou  robotique  qui  opere  et 
interagit  dans  les  milieux  complexes  et  non  structures  des  futurs  champs  de  bataille  ».  Une 
representation  exacte  du  monde  est  un  besoin  essentiel  du  domaine  de  F  intelligence  autonome. 

Un  vehicule  autonome  cree  une  representation  du  monde,  analyse  cette  demiere  et  note 
l’endroit  des  obstacles.  Un  tel  vehicule  est  en  mesure  d’eviter  les  dangers  et  de  manceuvrer  a 
travers  un  terrain  ne  comportant  pas  destructions.  La  representation  du  monde  la  plus 
commune,  la  carte  morphographique,  utilise  normalement  des  dispositifs  de  perception 
«  d’echantillon  unitaire  »  ou  des  implementations  de  «  balayage  a  F  arret ».  Ces  vehicules  en 
motion  continuelle  de  RDDC  exigent  une  technique  de  carte  morphographique  pouvant 
fusionner,  dans  une  carte  unique  coherente,  de  multiples  echantillons  de  terrains  acquis  sous 
differentes  poses. 

Conclusions  principales  :  RDDC  a  etudie,  propose  et  implemente  une  nouvelle  technique  de 
fusion  des  donnees  dans  une  carte  morphographique.  Cette  technique  statistique  a  variance 
ponderee  minimise  de  maniere  optimale  les  effets  d’erreur  a  F arret  creant  ainsi  une  carte 
moiphographique  coherente  a  partir  de  balayages  multiples  du  terrain.  On  a  compare  le 
rendement  de  la  technique  statistique  a  variance  ponderee  avec  celle  d’une  technique 
statistique  classique  et  on  a  determine  qu’elle  etait  superieure  a  cette  demiere. 

La  portee  des  resultats  :  RDDC  a  implemente  la  carte  moiphographique  a  variance  ponderee 
dans  le  cadre  d’une  architecture  d’echange  multimedia  d'objets  extraits  (MIRO).  Les  tests 
effectues  sur  le  Vehicule  terrestre  sans  pilote  Raptor  ont  revele  que  Fimplementation  efficace 
de  la  carte  n’exigeait  que  peu  de  ressources  calculatrices.  Une  demonstration  conduite  durant 
Fautomne  2005  a  mis  les  capacites  de  la  carte  a  Fepreuve.  Durant  ces  essais,  la  carte 
moiphographique  de  RDDC  a  reussi  a  produire  une  representation  adequate  du  monde.  La 
carte  est  demeuree  egocentrique  alors  que  le  Raptor  effectuait  des  manoeuvres  de  translation  et 
de  rotation.  La  carte  morphographique  a  joue  un  role  critique  permettant  au  Vehicule  terrestre 
sans  pilote  de  reussir  a  eviter  les  obstacles  et  les  dangers  tout  en  naviguant  vers  un  but  de 
maniere  autonome. 

Les  resultats  futurs  :  Bien  que  Fimplementation  actuelle  de  la  carte  soutienne  des  donnees 
3-D  provenant  a  la  fois  des  appareils  de  telemetrie  par  laser  et  des  cameras  de  stereovision,  la 
demonstration  des  Systemes  terrestres  autonomes  dependait  exclusivement  des  donnees 
provenant  des  lasers.  L’ implementation  de  la  stereovision  de  RDDC  n’ etait  pas  suffisamment 
evoluee  et  produisait  souvent  des  cartes  disparates  souffrant  d’ effets  de  franges  de  distorsion. 
Sans  les  hautes  densites  de  donnees  3-D  produites  par  la  stereovision,  la  carte 
moiphographique  souffrait  quelquefois  de  manque  de  donnees.  Les  travaux  futurs  etudieront 
les  moyens  de  minimiser  Fimpact  des  effets  de  franges  de  distorsion.  La  carte  morphologique 
actuelle,  implementant  une  representation  du  monde  en  2, D  ne  peut  pas  representer  des 
obstacles  et  des  dangers  ayant  des  proprietes  tridimensionnelles.  Des  obstacles  tels  que  les 
encorbellements,  les  tunnels  et  les  voies  de  passages  ont  des  restrictions  en  rapport  avec  les 
hauteurs  pouvant  interdire  une  navigation  securitaire.  La  recherche  future  etudiera  des 
implementations  multiples  de  cartes  qui  devraient  permettre  la  representation  en  3-D  des 
obstacles. 

G.S.  Broten,  J.L.  Giesbrecht,  S.P.  Monckton.  2005.  World  Representation  Using  Terrain  Maps. 
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1.  Introduction 


To  autonomously  navigate  rough,  unstructured  outdoor  environments  an  unmanned 
vehicle  (UxV)  must  create  an  accurate  representation  of  its  world.  The  most  common 
form  for  a  world  representation,  the  terrain  map  [1,  2,  3,  4,  5,  6],  requires  three 
dimensional  sensing.  Active  and  passive  ranging  sensors  have  been  extensively 
investigated  and  used  for  this  purpose.  Herbert  [7]  surveys  range  sensing  technologies 
including  laser  rangefinders  (LRF),  triangulation  rangefinders  and  passive  stereo  vision. 

The  data  from  perception  sensors  is  processed  and  fused  a  terrain  map,  creating 
coherent  world  representation.  This  world  represention  is  a  critical  element  required  for 
global  navigation  and  for  obstacle  avoidance. 

This  report  investigates  terrain  maps  relying  on  nodding  2-D  laser  rangefinder  (LRF) 
data.  The  quality  of  a  nodding  laser’s  3-D  data  depends  on  the  accuracy  of  the  pose 
estimation,  as  the  conversion  from  range  to  3-D  data  is  especially  sensitive  to  the  pose’s 
roll,  pitch  and  yaw  components  [8].  Roll,  pitch  and  yaw  measurement  errors  can  result 
in  3-D  positional  errors  with  magnitudes  equal  to  or  larger  than  the  autonomous 
vehicle’s  platform  size.  This  research  proposes  a  variance  weighted  statistical 
technique  to  optimally  fuse  3-D  range  data  into  a  terrain  map. 

This  report  is  organized  into  8  Sections.  Section  2.  defines  the  terrain  map’s  co-ordinate 
system.  Section3.  introduces  the  wrappable,  rotating  terrain  map  concept.  Fusing  3-D 
into  the  terrain  map  is  covered  in  Section  4.,  and  Section  5.  derives  generic  terrain  map 
requirements.  Section  6.  details  DRDC’s  specific  terrain  map  implemention. 
Experimental  results  are  presented  in  Section  7.1  and  the  report  finishes  with  the 
conclusions  presented  in  Section  8. 

2.  Definition  of  Co-ordinate  Systems 


Any  map,  used  for  navigational  purposes,  requires  a  co-ordinate  system.  Under  local 
navigation  circumstances  DRDC  defines  and  uses  an  egocentric  co-ordinate  system. 
Figure  1  shows  this  egocentric  co-ordinate  system.  This  co-ordinate  system  defines  the 
X  axis  as  parallel  to  the  forward  motion  vector  and  the  Z  axis  along  the  gravity  vector. 
The  Y  axis  is  subsequently  defined  using  the  righthand  rule.  Roll  occurs  about  the  X 
axis,  pitch  is  about  the  Y  axis  and  yaw  occurs  about  the  Z  axis. 

3.  Terrain  Maps  for  Navigation 


UxV’s  employ  many  navigation  strategies  based  upon  a  variety  of  assumptions  with 
respect  to  their  operational  environment  [9].  Early  research  followed  the  sense,  model, 
plan  and  act  paradigm  (SMPA)  [10,  11],  where  its  environment  was  structured, 
predictable  and  assumed  to  be  known.  Real  environments  are  unstructured  and 
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Forward  Motion 


Figure  1:  Map  Co-ordinate  System 


unpredictable,  thus  the  deliberative  SMPA  approach  performs  poorly.  The  reactive 
school  of  thought,  pioneered  by  Brooks  [12,  1 1],  used  the  environment  itself  as  the 
world  model  for  the  robot.  This  approach  resulted  in  robust  behaviour  for  unstructured 
environments,  but  it  did  not  yield  useful  applications  due  the  inability  to  direct  the 
behaviour  of  the  robot. 

Current  robotic  research  exploits  both  the  SMPA  and  deliberative  approaches  by 
implementing  a  hybrid  strategy.  Like  the  SMPA  paradigm,  the  hybrid  approach 
requires  the  robot  create  a  world  representation.  This  research,  with  its  control  strategy 
based  upon  the  hybrid  approach,  uses  a  grid  map,  formed  by  a  rectangular  array  of 
regions,  as  its  world  representation. 

The  simplest  grid  map,  the  occupancy  grid ,  represents  regions  as  either  occupied  or 
empty  and  is  suitable  for  flat  indoor  environments.  In  unstructured  outdoor 
environments  a  terrain  map  1  [3,  4,  1,2,  5]  captures  the  terrain  elevation  of  each  grid  in 
the  map.  The  grid  map,  shown  in  Figure  2,  illustrates  the  physical  form  and 
arrangement  of  a  terrain  map.  Each  grid  element  contains  the  terrain  mean  elevation,  z, 
and  the  elevation  variance  a2 . 


ZijCfj 


Figure  2:  Terrain  Map  Representation 

A  terrain  map  is  capable  of  representing  obstacles  and  other  features  found  in  outdoor 
environments  such  as  hills,  slopes,  bumps  and  dips. 

'Terrain  maps  are  also  commonly  referred  to  as  digital  terrain  maps,  digital  elevation  maps  or  2^D  grid  maps. 
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3.1  Terrain  Map  Size 


Sensor  characteristics  limit  terrain  map  size.  Assume  a  vehicle  has  a  set  of  range 
sensors,  S  =  [Si . . .  S„\,  each  with  a  maximum  range  Sr,  and  a  horizontal  field  of  view 
Sa.  The  maximum  size  of  the  terrain  map  can  be  determined  using: 

(1)  Xmax  =  max(SR;),  Ymax  =  max(SR;  sin(-^)) 

assuming  all  sensors  scan  to  the  horizon.  Figure  3  shows  a  terrain  map,  with  a  grid  size 
Gj,  yields  a  map  with  dimensions  of 

(2)  =  Ny=^ 

G  i  Gi 

where  Nx  and  Ny  are  the  number  of  grid  elements  along  the  X  and  Y  axes  respectively. 


A i 


Figure  3:  Terrain  Map  sized  to  Range  Sensing  Capabilities 

3.2  Wrappable  Map  Representation 

The  terrain  map  is  ego-centric,  fixing  the  map  in  the  vehicle  local  coordinate  system 
[13,  14].  Using  the  fixed  dimensions  of  Xmax  x  Ymax,  the  map  translates  and  rotates  the 
observed  world  using  actual  vehicle  motion.  For  this  map  the  X  coordinate  is  always 
parallel  to  the  vehicle’s  direction  of  movement.  Thus,  in  the  X  direction  of  motion,  the 
map  recycles  departing  elements  on  map  boundaries  into  new  elements  on  entering 
across  the  opposite  boundary,  wrapping  the  map  as  the  vehicle  moves.  Wrappable  maps 
are  highly  efficient  structures  where  a  simple  index  update  allows  the  map  to  track  the 
vehicle’s  forward  movement. 

3.2.1  Translation 

Figure  4  shows  the  simplest  wrappable  map  mode,  where  the  vehicle 
experiences  pure  translation.  The  variable  shown  in  Figure  4,  tracks  the 
current  index  into  the  wrappable  map. 
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Figure  4:  Wrappable  Map  Representation 


This  figure  illustrates  the  map  wrapping  and  grid  element  recycling  process 
for  a  vehicle  driving  along  a  straight  vector.  As  time  progresses  from 
t  =  0  — >  2,  the  map  index  m-,  moves  from  «?,■  =  0^2.  The  map  index  is 
dependent  upon  the  vehicle’s  motion.  Once  the  incremental  forward  motion  of 
the  vehicle  has  exceeded  the  grid  element  size,  Gp  map  index,  nip  is 
incremented,  thus  causing  the  map  to  wrap.  At  this  point  all  data  for  map 
elements  indexed  by  m,  —  1  or  greater2  are  denoted  as  invalid  since  this  index 
represents  terrain  that  has  already  been  traversed. 

3.2.2  Rotation 

A  holonomic  vehicle  spinning  on  its  axis  subjects  the  terrain  map  to  a  purely 
rotational  vector.  Figure  5  shows  the  rotation  vector,  0(7),  applied  to  the 
terrain  map. 


Figure  5:  Vehicle  Rotating  90  Degrees  About  Axis 


2  Multiple  grid  elements  may  have  been  traversed  during  the  pose  update  period. 
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For  rotational  motion  the  map  is  mathematically  transformed  by  the 
magnitude  of  the  vehicle’s  yaw  angle  and  the  map  data  is  shuffled  into  its 
corresponding  grid  location.  In  Figure  5  a  90°  rotation  is  applied  to  the  terrain 
map.  The  hatched  region  denotes  the  map  area  at  time  t  +  I  that  overlaps  with 
the  initial  map  at  time  t. 

3.2.3  Translation  and  Rotation 

Vehicles  operating  in  the  real  world  have  a  motion  vector,  M  that  includes  both 
a  translation  and  a  rotation  component.  Figure  6  show  a  terrain  map  subjected 
to  the  motion  defined  by  vector  Mt  =xtZQt,  over  the  time  period  t  =  0  — >  1. 


Figure  6:  A  terrain  map  with  translation  and  rotation 

The  motion  vector,  Mt,  can  be  decomposed  into  its  translation  and  rotation 
components,  thus  allowing  the  map  update  to  be  implemented  as  a  two  stage 
process. 


3.3  Indexing  the  Map 

To  ensure  persistence  while  suppressing  transients,  the  terrain  map  fuses  new  range 
data  along  with  previous  data  into  the  map’s  grid  elements.  The  range  data,  represented 
by  a  the  scan  point  (x)  in  the  laser  beam  co-ordinate  system  [8],  is  first  transformed  to  a 
3-D  data  point,  (v,y,z),  in  the  map  co-ordinate  system.  The  3-D  data  point’s  (x,y)  can 
then  be  transformed  into  the  map  grid  element  index,  (i,j),  using: 


(3) 


i  =  int 


'mod(yH 

_  syntax  7 

G, 
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(4) 


j 


int 


(mod(ytA 
V  J 


where  G\  defines  the  dimensions  of  a  grid  element,  and  Xmax,  Ymax  define  the  map  size. 
The  mod  operator  returns  the  floating  point  remainder  of  the  function  and  the  int 
operator  returns  the  integer  portion.  Figure  7  shows  this  transformation  from  the  scan 
point’s  (x,y)  to  the  map  index  (/,/). 


Figure  7:  X,  Y  Co-ordinate  to  Grid  Map  Index 


3.3.1  Indexing  the  Wrappable  Map 

Section  3.3  presented  the  equations  to  transform  a  scan  point  to  index  pair, 
(x,y)  — >  (/,  /),  for  the  static  map  situation.  This  Section  expands  these 
equations  to  support  the  indexing  of  a  wrappable  map,  as  given  below. 


(5) 

(6) 
(V) 
(8) 


i 


j 


ntj 


int 


t± ) 

V  J 


+  ntj 


j  i  if  1  <  Nx 
l  i~Nx  if  i>Nx 


int 


(m°d^YtA 
V  J 


(  Mi- 1  +  1  if  x(t)—x(t 
\  nti-i  otherwise 


1  )>G, 


where  m,  =  is  the  grid  map  index  in  the  X  direction.  The  transformation 
y  — >  j  remains  unchanged,  but  the  transformation  x  — >  i  is  modified  to  include 
the  grid  map  index  and  the  wrapping  situation.  Figure  8  shows  the  scan  point 
to  index  transformation  for  a  terrain  with  a  map  index  m,  =  1. 
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Figure  8:  X,  Y  Co-ordinate  to  Wrappable  Grid  Map  Index 

3.3.2  Map  Rotation 

As  shown  in  Figure  5  yaw,  given  by  0,  is  about  the  z-axis.  In  order  to  reduce 
the  computational  burden  associated  with  applying  a  transformation  for  each 
individual  yaw  update,  a  map  rotatation  occurs  at  a  fixed  angle.  When  the 
total  yaw,  Yl"=i  0o  exceeds  the  maximum  yaw  angle,  0,/,  a  map  yaw 
transformation  occurs.  Figure  9  shows  this  rotational  transformation. 


<M< ) 


Mt),j,l(t))  ->  Mt),ya(t))  Cw(<), 3W»(0)  -t  ( i„n;jnew ) 


Figure  9:  Coordinate  Transformation  due  to  Rotation 

The  first  step  of  the  transformation  uses  the  map  indices  (i.  j)  to  determine  the 
grid  element’s  position  coordinate. 

(9)  xei(t)  =  4/(f)-G/  +  y  +xre(t-\) 

(10)  yel(t)  =  jel(t)-G,  +  Y+yre{t-  1) 

The  remainders  from  the  previous  rotation,  given  by  xre(t  —  1)  and  yre(t  —  1), 
negate  the  quantization  effects  resulting  from  binning  the  (x,y)  location  into 
the  grid. 
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The  next  step  determines  the  vector,  Rei,  from  the  map  origin  to  the  grid 
element  position. 


(11) 

(12) 


Rel{t)  =  \Jxel(t)2+yel(t)2 

<M0  =  tan  ^ 

Xel  [t ) 


Rotating  the  vector,  Rci,  by  the  yaw  angle,  0j,  yields  the  new,  (xnew.ynew), 
grid  element  position,  as  well  as  the  remainders. 


(13) 

%new(t ) 

=  Rei(t)cos($ei{t)  +  Qd) 

(14) 

ynewif) 

=  Rei  (t )  sin(<|)g/  (t )  +  0</) 

(15) 

Xreit ) 

=  mod  (xnew(t),G,) 

(16) 

yreit) 

=  mod  (ynew(t),Gi) 

As  described  in  Section  3.3,  the  (xnew(t)  ,ynew(t))  coordinate  pair  converts  into 
a  map  index  pair,  (, inewjnew ),  which  becomes  the  holder  for  the  map  data 
previously  indexed  by  the  map  pair  ( ieujei )■ 

3.4  Optimizing  the  Wrappable  Map  Implementation 

The  wrappable  map’s  translation  component  is  not  computationally  demanding,  but  the 
rotational  component  has  the  potential  to  consume  large  amounts  of  processing  power. 
The  rotation  algorithm,  described  in  Section  3.3.2,  can  be  implemented  as  a  runtime 
calculation  or  it  may  be  implemented  using  a  lookup  table  map.  The  runtime 
calculation  requires  index  pair,  ( iehjei ),  the  grid  element  size  Gi  and  the  remainder 
from  the  previous  rotation,  (xre(t),yre(t)),  in  order  to  determine  the  grid  element 
position,  Rc].  Of  these  variables,  only  the  remainder  component,  (. xre(t),yre(t )),  is 
runtime  dependent.  Thus  a  lookup  table,  mapping  index  pairs,  ( iei,jel ),  to  grid  element 
positions,  xei(t),yei,  can  be  precalculated  as  shown  below. 


(17) 

Xel(t) 

=  iel(t)-Gl  +  ~Y 

(18) 

yei(t) 

=  jel(t)'Gi  +  -y 

This  lookup  table  reduces  the  calculations  required  during  the  runtime  phase. 
Additionally,  trigonometric  functions  such  as  cosine,  sine  and  tangent  are 
computationally  expensive.  A  trigonometric  lookup  table  would  also  ease  the 
computational  burden. 
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3.5  Pose  Errors 


If  the  grid  element’s  variance,  G~  or  a;  exceeds  the  grid  element  size,  Gi,  this  grid 
element’s  data  should  be  fused  into  all  affected  neighbouring  elements.  Figure  10 
shows  grid  element  (i,j),  with  a  mean  elevation,  Z(;j),  and  variance,  a~  .  ^  =  crv;  /),  do 
not  affect  neighbouring  grid  elements.  Conversely,  grid  element  (i  +  2,j),  with  a  ax 
variance  of  a^;+2  and  ay  variance  of  a^;_2  /  t  influences  two  of  its  neighbours. 


Figure  10:  Scan  Point  Fusion  into  Grid 


3.6  Grid  Element  Size 

Traditionally  a  terrain  map’s  grid  element  size  corresponds  to  the  minimum  obstacle 
size  that  can  be  safely  traversed  by  the  vehicle.  With  a  persistent  terrain  map  it  is 
desirable  to  scan  the  terrain  as  often  as  possible,  since  more  sample  data  yields  a  better 
statistical  representation  of  the  world  [8]. 

Although  it  is  desirable  to  sample  as  often  as  possible,  perception  sensors,  such  as 
stereo  vision  devices  and  laser  range  finders,  have  limited  sampling  rates.  The  update 
rate  for  stereo  vision  devices  is  limited  by  the  camera  shuttle  speed  and  the  available 
processing  power.  For  a  nodding  LRF  the  sampling  frequency  is  dependent  upon  the 
nodding  profile;  for  the  uniform  sample  distance  profile  the  sampling  frequency  is 
inversely  related  to  the  sample  distance  and  for  a  constant  rotational  rate  the  sample 
frequency  is  fixed.  [8],  The  preferred  uniform  sample  distance  profile  can  match  the 
sample  distance  with  the  map’s  grid  element  size.  This  size  matching  is  intuitive  since 
a  sample  distance  smaller  than  the  grid  size  results  in  empty  grid  elements,  and  a  larger 
sample  distance  results  in  a  loss  in  fidelity.  Selecting  a  sample  distance  smaller,  than 
the  required  minimum,  is  undesirable  since  it  reduces  the  sampling  frequency.  Thus,  a 
persistent  terrain  map  should  maximize  the  sampling  frequency  while  selecting  a  grid 
element  size  that  is  appropriate  for  the  vehicle’s  capabilities. 
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4.  Terrain  Map  Creation  using  3-D  Data 


Any  3-D  data  sources  may  be  used  by  a  terrain  map,  including  stereo  vision  data,  and 
the  3-D  data  returned  from  a  LRF.  For  a  vehicle  operating  under  continuous  motion 
conditions3,  a  persistant  terrain  map  must  fuse  range  data  where  each  terrain  scan  is 
acquired  at  a  different  vehicle  pose.  A  vehicle  travelling  at  3  m/s  will  require  10 
seconds  to  traverse  the  30  m  of  terrain  represented  by  a  map.  Thus  the  terrain  map  must 
fuse  new  3-D  data,  acquired  over  a  significant  time  period,  while  minimizing  the  effects 
of  pose  errors. 


4.1  Confidence  in  the  Range  Data 

The  confidence  level  associated  with  range,  expressed  as  a  statistical  variance,  is  a 
function  of  both  the  range  sensor  and  the  vehicle  pose  accuracy.  The  vehicle  pose  has 
two  components,  the  position  and  the  orientation.  The  ALS  project  uses  a  NovaTel 
DL-4-RT2  DGPS  receiver  with  a  position  accuracy  of  ±3  cm.  The  Microstrain 
3DM-GX1  IMU  provides  the  vehicle’s  orientation  to  an  accuracy  of  ±2  degrees.  The 
relationship  defining  the  range  data’s  variance  must  be  derived  for  each  individual 
range  sensor  type. 

4.1.1  Nodding  SICK  Laser 

The  nodding  SICK  laser  is  the  primary  perception  sensor  for  the  ALS  project. 
As  derived  in  ’’The  Characterization  of  an  Inexpensive  Nodding  Laser”  [8], 
orientation4  is  the  most  significant  error  source  affecting  the  laser’s  accuracy. 
The  SICK  laser’s  range  reading  defines  a  point  in  space  that  is  encircled  by  a 
spherical  error  region.  This  error  is  proportional  to  both  the  range  and  the 
estimated  orientation  error.  Equation  19  shows  the  range/orientation 
relationship  for  the  SICK  laser. 


(19) 


where  r  is  the  range  and  Ae  is  the  estimated  error  in  orientation.  The  sphere’s 
radius,  A.v2,  defines  the  region  that  bounds  the  true  position  of  the  range 
reading.  Thus  As  can  be  interrupted  as  the  variance  associated  with  the  range 
reading. 


(20)  G2  =  As2 

3l.E.  The  UxV  platform  does  not  allow  for  a  stop  and  scan  configuration 
4Given  by  roll,  pitch  and  yaw 
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4.2  Fusion  of  Data 


A  grid  based  terrain  map  implementation  fuses  range  data  into  the  appropriate  grid 
element,  as  derived  in  Section  3.3.1,  using  a  statistical  approach.  The  data  fusion 
process  takes  individual  range  3-D  position  readings  and  calculates  the  first  and  second 
moment  statistics  for  the  grid  element.  Thus  a  grid  element  represents  terrain  using  the 
average  elevation,  z.  The  confidence  in  this  elevation  value  is  given  by  the  variance,  a2, 
associated  with  this  terrain  patch. 

Figure  1 1  illustrates  the  data  fusion  process,  showing  grid  element  (z^j) ,  o2  .  ^ )  with  a 
small  variance  is  more  accurate  than  grid  element  (z(;+2j) •  °2(,-+2  j))- 


G, 


Figure  11:  Range  Data  Fusion  into  a  Grid  Element 

This  data  fusion  approach  is  not  tied  to  a  single  statistical  technique,  thus  the  researcher 
is  free  to  experiment  with  different  implementations. 

4.2.1  Classical  Statistics 

The  traditional  technique  to  fuse  new  3-D  range  data  into  a  terrain  map  is 
based  upon  first  and  second  order  moment  statistics  [4],  The  classical  mean 
and  variance  is  given  by: 


(21) 

(22) 


T  = 


cr  = 


N 

-V N 


Si  fr-X 

N-  1 


^2 


Although  classical  statistics  have  been  successfully  implemented  on  terrain 
maps,  they  do  not  use  geometry  and  error  source  knowledge  and  thus  are  a 
sub-optimal  implementation. 
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4.2.2  Variance  Weighted  Statistics 


Classical  statistics  do  not  build  upon  error  sources,  mapped  through  the 
physical  geometry  of  the  system,  when  fusing  data  into  the  terrain  map.  As 
described  in  Section  4.1.1,  the  orientation  error  when  mapped  through  the 
geometry  of  a  nodding  laser,  yields  an  estimate  of  the  range  point’s  variance. 
Thus  a  technique  is  required  that  optimally  combines  data  with  differing 
variances.  The  ALS  project  implemented  a  variance  weighted  statistical 
technique  to  optimally5  fuse  new  3-D  range  data  into  the  map.  The  variance 
weighted  mean  is  given  below: 


(23) 


xj_ 

■2-2=1  q2 
_L 

22=1  aj 


It  can  easily  be  calculated  on-line  without  the  need  to  store  all  data  points 
using: 


EtV+1  Zj_ 

_  i=\  a1 

(24)x,-+i  =  N+l  ^ 

22=1  c2 


xi  |  ^2  i  ^3  |  |  x n  i  'fyv+i 

^2  l  '  *2  '  ’  *  *  '  I  _o 

°1  °2  °3  °N  °N+l 


X)V+ 1  Ti_  .  XW+1 

nl  .  2-^i=  1  n?  al  . 


ill  i  1...1  i  _ 

-2  '  -2  i  n2  r-2  l  fJl 

°2  °3  °N  °iV+ 1 


1  1 
d 2^i=  1  a2  f  ~2 


Thus  the  new  variance  weighted  mean  can  be  simply  calculated  from  the 
previous  value  and  the  new  data.  The  variance  weighted  mean  requires  several 
sums  be  maintained: 

1.  The  number  of  sample  points:  N 

2.  Sum  of  squared  sample  over  variance  squared:^^  ^ 

3.  The  sum  of  the  variances ^2 

The  definition  of  variance  weighted  variance  is: 


(r,-x)2 
2-2=1  c2 

_j_ 

22=1  a2 

Equation  25  is  not  a  computationally  efficient  form.  Expanding  this  equation 
results  in: 

5  Variance  weighted  means  are  the  underlying  premise  of  Kalman  Filtering 


(25) 


a2  = 
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(26) 


Y^W  t?-2t,t+x2 
2X=  1  X 

TT N  1 
2—ii=l  X 


EN  xj_  _  2XjX  ,  v-^'V  xr_ 

i=  1  X  2_w=l  X  T  2_w=  1  X 

_j_ 

2x=l  a2 


The  middle  term  can  be  simplified  as  follows: 


Y^iV  iXjX  -J-  Y^JV  Xj_  /  Y^iv  X. 

2x=l  a2  ZL2_w'=  1  a2  2X=1  a2 

Y^W  J_  yX^  _L  I  Y^w  _L 

2X=1  X  2X=1  X  \  2X-I  X 


Xi 


Remembering  the  definition  of  the  variance  weighted  mean,  the  above 
simplifies  to: 


2XjX 

i=  1  (Jr  __ 

- —  =  2xx  =  2x~ 

Y-W  J_ 

2X=1  X 


Substituting  back  into  this  expanded  equation  and  noting  that  x2  is  a  constant 
for  any  given  index  results  in: 


yN ,  X  yt 

(29)  a2  = _ —  -  2x2  q _ 

['y>  u  Y^w  _L  ^Y _L 

2_-//— 1  a2  2x=l  X 


Y^w  yt_ ,  =2  j_ 

2x=l  a2  1  2_y/=  1  a2 

- - —  2tz  -I - — 

i  Z'L  ^  _L 

Z— //=1  q2  Z— //=!  (j2 


Rationalizing  for  a  common  denominator  gives: 


(30)  a2  = 


yN  \_2x2 yN_.  \  +  x2yN  \ 

/  Jl — 1  (jf  /  Jl — 1  (j2  /  ji — \  ^2 

Y^W  _j_ 

2x=l  X 


Y^w  2L  _  t2  -L 

2^(=1  (j2  L  2^(=  1  a2 

Y^A'  _j_ 

2X=1  X 


Once  again  using  the  definition  of  the  variance  weighted  mean  and  factoring 
the  common  elements  yields: 


(31)  a2  = 


(y^N  ii  \  2 

^'=1  j?  I  x 

EYyJ  ^'=lc- 

Y^W  X 
2x=l  X 


Y^w  x  _ 
2x=l  x 


xj 

Xi=!  „2 


1 

1  „2 


Y^«  X 

2x=i  X 


Rationalizing  to  a  common  denominator  yields  the  equation  in  its  final 
computing  form: 
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(32) 


a 


2 


l^i=  1 


cj  2-1  i=  1  {j2  \^2_/(=  1  c2  ^ 


For  the  computing  form  of  the  variance  weighted  variance  there  are  only  three 
sums  that  must  be  maintained: 

1.  Sum  of  squared  variances:  =  Xwll1  ^2  +  ;jr 

2.  Sum  of  squared  sample  over  variance  squared:  JjfLi  ^  ^ 

3.  Sum  of  sample  over  variance  squared:^^  ^  ^ 

These  sums  can,  calculated  in  a  manner  analogous  to  variance  weighted  mean, 
define  the  complete  computing  form  for  variance  weighted  statistics. 

5.  Terrain  Map  Requirements 


The  previous  sections  of  this  report  detailed  the  theoretical  aspects  of  DRDC’s  terrain 
map  implementation.  This  implementation  efficiently  collates  large  data  sets  using  a 
variance  weighted  statistical  approach.  This  new  approach  retains  fidelity,  yet  is 
applicable  to  real-time  operations. 

The  terrain  map  uses  statistics  to  summarize  the  terrain  properties  associated  with  each 
grid  element.  The  statistical  values  include: 

1.  The  mean  x  position,  x  and  the  variance  a~. 

2.  The  mean  y  position,  y  and  the  variance  G“. 

3.  The  mean  and  the  variance  in  the  z  elevation  given  by  z  and  0?. 

A  minimal  map  implementation  requires  the  z  statistics.  The  x  and  y  statistics  are 
optional  values  that  can  be  used  in  more  advanced  map  implementations  as  given  in 
Section  3.5 


5.1  Calculation  of  the  Mean  and  Variance 

The  (x,y,z)  averages  and  variances  are  calculated  using  variance  weighted  statistics  as 
detailed  in  Section  4.2.  Table  1  shows  the  10  sums  and  quantities  required  in  the  online 
calculation  of  the  (x,y,z)  variance  weighted  statistics. 
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Number 

Description 

Sum  or  Quantity 

1 

Sample  Size 

N 

2 

Sum  of  squared  x  variances 

j 

2^=1  o2- 

JCl 

3 

Sum  of  squared  x  over  variance  squared 

y^ N  A 

2-ji=l  a2 

XI 

4 

Sum  of  x  over  variance  squared 

Xi 

2-^=1  (j2 

XI 

5 

Sum  of  squared  y  variances 

y47V  I 

2^i=l  a2 

6 

Sum  of  squared  y  over  variance  squared 

yf 

^ i=1  A, 

7 

Sum  of  y  over  variance  squared 

sr^N  ft 

2^i=l  a2 

yi 

8 

Sum  of  squared  z  variances 

y^A/  1 

4 

9 

Sum  of  squared  z  over  variance  squared 

y^v  A 

2^=i  C2 

10 

Sum  of  z  over  variance  squared 

y^A/  t 

2^=1  a2 

_ Zl _ 

Table  1:  Sums  and  quantities  of  a  Map  Element 


5.2  Memory  Requirements 

The  memory  required  to  implement  a  grid  based  terrain  map  is  dependent  upon: 

1.  The  grid  element  size. 

2.  The  map  size. 

3.  The  sums  and  quantities  associated  with  each  element  of  the  grid. 

The  number  of  grid  elements  in  a  terrain  map  is  derived  in  Section  3.1  and  is  given  by 
Nx  x  Ny.  The  number  of  sums  and  quantities  for  each  grid  element  is  Nsums  =  10,  as  is 
shown  in  Table  1.  Thus  the  memory  required  is  given  by: 

(33)  Mr  =  Ny  x  Ny  x  Nsums 

Assuming  the  nodding  SICK  laser,  with  a  range  of  30m,  as  the  primary  perception 
sensor,  and  a  grid  element  size  G /  =  0.2,  Section  3.1  yields  Nx  =  Ny  =  =  150. 

Thus  the  memory  requirements  for  the  terrain  map  can  be  calculated  as: 

(34)  Mr  =  NxxNyx  Nsums  =  150  x  150  x  10  =  225000 

If  each  sum  or  quantity  is  assumed  to  be  an  8  byte  floating  point  number  then  a 
approximately  2,000,000  bytes  of  memory  is  required.  This  value  is  readily  achievable 
for  current  computing  platforms. 
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6.  Implementing  a  Terrain  Map  Under  MIRO 


The  ALS  project  implemented  a  variance  weighted  terrain  map  under  the  Miro 
framework  [15,  9,  16,  17,  18].  This  map,  representing  the  terrain  in  front  of  the  Raptor 
UGV,  feeds  the  traversibility  analysis  and  thus  is  instrumental  in  enabling  obstacle  and 
hazard  avoidance  behaviours.  The  Raptor  UGV  is  shown  in  Figure  12. 


Figure  12:  The  Raptor  UGV 


6.1  Configuration 

The  Raptor  UGV  carries  four  perception  sensors  that  feed  data  to  the  terrain  map: 

•  Nodding  SICK  Laser. 

•  Digiclops  Stereo  vision  cameras. 

•  Sokkia  GSR2600  DGPS. 

•  Microstrain  3DM-G  IMU. 

The  nodding  SICK  laser,  operating  at  a  26ms  update  rate,  produces  range  data;  the 
Digiclops  camera  updates  its  disparity  map  every  500ms.  The  Sokkia  GPS  provides  a 
new  position  every  250ms  and  the  Microstrain  IMU  produces  an  orientation  at  62.5ms 
intervals. 

The  ModelServer  [19]  component  fuses  the  GPS  position  with  the  IMU  orientation  to 
publish  a  pose  event  once  every  100ms. 

DRDC’s  computationally  efficient  terrain  map  implementation  fuses  data  and  performs 
map  wrapping  at  the  perception  sensor  data  rates.  But,  a  map  event  is  only  published  on 
500ms  intervals  since  the  computationally  intensive  traversibility  analysis  can  not 
handle  higher  data  rates. 
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6.2  The  Terrain  Map  Component 


The  MIRO  framework  [17]  prompts  component  based  implementations,  where  a 
component  represents  a  unique  capability  or  behaviour.  Components  share  data  via 
network  transparent  CORBA  objects  [20,  21].  The  Terrain  Map  component  received 
and  publishes  CORBA  events,  therefore  the  Publish-Subscribe  Server  design  pattern 
served  as  the  template  for  this  implementation  [16].  The  collaboration  diagram,  shown 
in  Figure  13,  illustrates  the  Terrain  Map  component. 


:  EventChannel  :  EventChannel  CORBA  :  EvenlChannel 


Terrain 

T 

:  notifyEventO 

Miro 

1 

|  :  Dispatcher  | 

|  1  Laser  Range3d 

Stereo  Range3c 

T 

j  1  :  push_structured_event() 

:  push_structured_event() 

setDataO 

it  i 

1 - r- 

l  :  Maplmp 

h 

:  makeMapO 

J. 

:  integrateData!) 

i 

:  checkScroUMap!) 

|  :  TerrainPose  | 

Figure  13:  Map  Publish-Subscribe  Server  Design 


The  Terrain  Map’s  classes  are  described  in  the  following  sections. 

6.2.1  RangeConsumer 

Upon  initialization  the  RangeConsumer  class,  using  the  CORBA  Naming 
Service,  resolves  and  subscribes  to  Laser  and  Stereo  CORBA  Range3d 
events,  as  well  as  Pose  CORBA  events. 

Under  operational  conditions  the  RangeConsumer  consumes  Range3d 
CORBA  events  and  invokes  the  makeMap  method,  which  integrates  the  new 
3-D  range  data  into  the  terrain  map.  Once  the  500ms  update  interval  has 
expired  the  RangeConsumer  calls  the  integrateData  method,  the  first  step  in 
publishing  a  Map  CORBA  event. 

When  the  RangeConsumer  receives  a  Pose  CORBA  event  it  calls  the 
checkScroUMap  method. 
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6.2.2 


TerrainMap 


The  TerrainMap  class  fuses  new  3-D  data  into  the  terrain  map  using  variance 
weighted  statistics.  Before  data  fusion  occurs,  the  range  is  first  transformed 
from  its  local  coordinate  system  to  the  map  coordinate  system.  This 
transformation  is  accomplished  with  assistance  from  the  ModelServer  and  its 
transformation  methods  [19]  and  results  in  a  3-D  data  point.  With  the  range 
data  transformed  into  the  appropriate  coordinate  system,  the  mapping 
algorithm  fuses  then  new  data  into  the  current  terrain  map. 

6.2.3  TerrainPose 

As  described  in  Section  3.2  the  vehicle’s  motion  vector  has  two  components, 
translation  along  the  X  axis  and  rotation  about  the  Z  axis.  The  TerrainPose 
class  implements  two  methods,  corresponding  to  the  translation  and  rotation 
components.  Both  of  these  methods  are  invoked  from  the  checkScrollMap,  as 
shown  in  Figure  13. 

The  translation  method,  scrollMapX,  wraps  the  map  when  the  vehicle’s 
incremental  forward  motion  exceeds  the  grid  size,  G/.  The  wrapping  process 
increments  the  grid  map  index,  m;,  and  initializes  the  traversed  grid  elements. 

The  rotation  method,  scrollMapY 6  implements  a  map  rotation  once  the 
summed  vehicle  yaw  exceeds  the  deviation  angle,  0,/.  To  simplify  the 
implementation  and  improve  performance  the  rotation  method  uses  a  working 
map.  The  rotation  algorithm  blindly  applies  the  rotation  geometry  to  each  grid 
element,  determines  the  map  indices  resulting  from  the  rotation,  and  sums  the 
grid  statistics  into  the  working  map.  The  working  map  then  becomes  the  new 
current  map  and  the  previous  current  map  becomes  the  new  working  map. 

6.2.4  Maplmp 

The  integrateData  method,  in  the  Maplmp  class,  copies  the  map  data  into  a 
locally  allocated  map.  Using  this  local  map  the  Terrain  Map ’s  server 
implementation  responds  to  polling  requests.  Before  returning,  integrateData 
passes  the  Terrain  Map  CORBA  object  to  the  setData  method. 

6.2.5  Dispatcher 

The  Dispatcher  takes  the  Terrain  Map  CORBA  object  and  publishes  the 
CORBA  object  on  the  EventChannel. 

6This  method’s  name  is  misleading  since  it  implements  rotation 
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6.3  CORBA  Objects 


The  Terrain  Map  component  supports  two  CORBA  object  types: 

•  MapSeqEventIDL.  A  generic  map  object  based  upon  C++  sequences. 

•  MapArrayEventIDL :  A  fixed  length,  2  dimensional  array  optimized  for 
performance. 

The  IDL  code,  implementing  the  generic  map  object,  is  shown  below. 


//!  A  vector  of  floats  creates  a  Map  Row. 
typedef  sequence<double>  MapRowIDL; 

//!  A  vector  of  Map  Rows  creates  a  2D  Map  Grid, 
typedef  sequence<MapRowIDL>  MapGridIDL; 

//!  A  vector  of  Map  grids  creates  the  full  3D  map. 
typedef  sequence<MapGridIDL>  MapSeqlDL; 

struct  MapSeqEventIDL 

{ 

//!  The  time  the  map  was  acquired. 

TimelDL  time; 

//!  The  Pose 
PoseTransf ormIDL  pose; 

//!  Define  the  Map  type 
long  maptype; 

//!  Depth  of  the  map  as  an  index  =  depth/gridsize. 
long  index_depth; 

//!  Width  of  the  as  an  index  =  2*width/gridsize 
long  index_width; 

//!  A  pointer  to  the  current  starting  X  index  in  the  map 
long  x_curr; 

/ / !  The  map. 

MapSeqlDL  map; 


The  IDL  code,  creating  the  fixed  size  CORBA  map  object,  is  shown  below. 


//!  For  the  purposes  of  speed  create  a  fixed  length  array  map 
typedef  double  MapArrayIDL  [  ZVAR+1  ]  [  INDEX_DEPTH  ]  [  INDEXJ/IIDTH  ]  ; 

struct  MapArrayEventIDL 

{ 

//!  The  time  the  scan  was  acquired. 
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TimelDL  time; 

//!  The  Pose 
PoseTransf ormIDL  pose; 

//!  Define  the  Map  type 
long  maptype; 

//!  Depth  of  the  map  as  an  index  =  depth/gridsize. 
long  index_depth; 

//!  Width  of  the  as  an  index  =  2*width/gridsize 
long  index_width; 

//!  A  pointer  to  the  current  starting  X  index  in  the  map 
long  x_curr; 

/ / !  The  map  . 

MapArrayIDL  map; 


The  sequenced  based  CORBA  object  sacrifices  speed  for  improved  data  abstraction;  the 
fixed  size  array  based  CORBA  object  optimizes  performance. 


6.4  Debugging  and  Visualization  Tools 

The  QtMap  interface,  shown  in  Figure  14,  presents  a  plan  view  of  the  terrain  map. 
Regions  in  red  are  elevated,  and  yellow/green  regions  represent  flatter  terrain. 


Figure  14:  QtMap  Interface 

The  SensorddGet  and  Sensor 3 dStream  debugging  components  print  the  mean 
elevation,  z,  and  the  variance,  G2,  for  each  grid  in  the  terrain  map. 

6.5  Maintaining  an  Egocentric  Map 

The  terrain  map  is  egocentric,  as  described  in  Section  4,  and  is  referenced  to  the 
Raptor’s  front  bumper.  The  terrain  map  uses  the  pose  estimate,  supplied  by  the 
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ModelServer  component,  to  maintain  the  map’s  egocentricity.  The  world  pose  estimate 
is  transformed  to  the  map's  coordinate  system  using  a  relative  pose  change  approach. 
As  shown  in  Equation  36,  the  change  in  pose  it  calculated  every  time  a  pose  update  is 
received.  This  change  in  pose  change  transforms  the  data  from  world  coordinates,  to 
the  egocentric  coordinate  system  required  by  the  map. 


(35)  Axm  =  \J  ((x, -xt-i  )2  +  (yt -yt- 1)2) 

(36)  A0m  =  0,  —  0f-i 


Where  x,  and  yt  are  the  estimated  position,  and  0,  is  the  estimated  heading,  both  in 
world  coordinates.  Axm  and  A0,„  define  the  incremental  position  change  in  map 
coordinates. 


6.6  Performance 

The  terrain  map  proved  to  be  one  of  the  more  computational  demanding  components  in 
the  Raptor  UGV  system.  The  resources  7  required  to  process  laser  3-D  data  events  and 
stereo  3-D  data  events  are  shown  in  Table  2. 


CORBA  Object 

Update  Period 

CPU% 

Memory  % 

Range  3  dLaserEventIDL 

26.6ms 

2.3 

2.8 

Range3dStereoEventIDL 

500ms 

9.5 

2.5 

Table  2:  Computational  Resources  Consumed 


7.  Results 

7.1  Experimental  Results 

The  performance  of  a  terrain  map  based  on  classical  statistics  was  compared  to  a 
variance  weighted  mean  terrain  map.  For  this  experiment  the  nodding  apparatus  was 
mounted  at  a  height  of  0.87  m  above  a  flat  floor  and  the  nodding  mechanism  was 
commanded  to  create  a  uniform  scan  at  a  density  of  10  cm  per  scan.  Each  scan  point’s 
raw  range  data,  encoder  position  and  time  synchronization  was  acquired  and  written  to 
file. 

Matlab  implemented  the  terrain  map  data  fusion  algorithm  using  both  classical  and 
variance  weighted  statistics.  Using  each  technique  the  scanned  flat  floor  was 
represented  using  a  terrain  map  representation.  Figure  15  shows  the  calculated  floor 
height  versus  distance  from  the  laser,  for  a  slice  of  the  terrain  map  at  a  Y  co-ordinate  of 
0  ±5cm. 

7For  a  Pentium  2.8  GHz  processor  with  1  GB  of  RAM 
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Z  Height  versus  X  Co-ordinate  (along  Y  axis  =  0) 


Figure  15:  Floor  Height  vs  Distance  from  Robot 


As  can  be  seen  in  this  figure  the  z  elevation’s  accuracy  varies  with  the  range  from  the 
laser.  This  experiment  was  performed  with  and  without  IMU  data.  Table  3  compares 
the  two  statistical  technique’s  performance.  For  each  technique  the  mean  elevation,  z, 
and  the  variance,  a2,  is  shown. 


Data  Fusion  Technique 

z(m) 

CT2  (mm) 

Classical  Statistics 

0.0060 

0.035 

Variance  Wt  Statistics 

0.0055 

0.029 

Classical  Statistics  with  IMU 

0.0015 

0.038 

Variance  Wt  Statistics  with  IMU 

0.0015 

0.031 

Table  3:  Results  scanning  a  flat  floor 


This  table  shows  both  techniques  produce  excellent  results,  with  the  variance  weighting 
producing  a  slightly  more  accurate  flat  floor  representation.  The  inclusion  of  the  IMU 
orientation  data  into  the  calculations  did  not  significantly  affect  the  results,  which  is  not 
suprising  since  the  Microstrain  3DM-G  IMU,  under  static  conditions,  exhibited  a 
standard  deviation  of  0.047  degrees  from  0  degrees. 


7.2  Simulations 

Although  IMUs  are  accurate  under  static  conditions,  their  performance  under  dynamic 
conditions  are  typically  worse  by  orders  of  magnitude.  A  vehicle  traversing 
unstructured,  outdoor  terrain  will  place  the  IMU  in  a  dynamic  environment  and  thus  it 
is  important  to  characterize  the  performance  of  terrain  map  creation  techniques  under 
these  conditions.  The  Microstrain  3DM-G  specifies  an  accuracy  of  ±5  degrees  while 
operating  under  dynamic  conditions  and  the  newer  Microstrain  3DM-GX1  specifies  an 
accuracy  of  ±2  degrees.  A  simulation,  performed  using  the  real  flat  floor  data,  added 
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random  noise  to  the  IMU’s  orientation  data  to  simulate  the  accuracy  of  the  Microstrain 
IMUs  under  dynamic  conditions.  The  performance  of  each  data  fusion  technique  is 
shown  in  Table  4. 


Data  Fusion  Technique 

IMU 

z  (m) 

a2  (mm) 

Classical 

3DM-G 

-0.211 

25.7 

Variance  Wt 

3DM-G 

-0.182 

18.7 

Classical 

3DM-GX1 

-0.077 

3.9 

Variance  Wt 

3DM-GX1 

-0.068 

2.9 

Table  4:  Results  a  flat  floor  with  simulated  Microstrain  Accuracies 


The  results  in  Table  4  reveal  the  variance  weighted  technique  is  significantly  less 
sensitive  to  IMU  errors  than  is  the  classical  statistics  technique. 


7.3  Demonstation  Results 

The  ALS  demonstrations,  conducted  in  September  2005,  extensively  exercised  the 
terrain  mapping  component.  A  typical  terrain  map  generated  using  the  nodding  SICK 
laser  is  shown  in  Figure  16.  The  map  extends  from  the  Raptor’s  front  bumper  to 
approximately  20m  in  front  of  the  vehicle  and  10  m  on  either  side  of  the  vehicle.  The 
red  region  on  the  map’s  right  hand  side  represents  a  building  wall,  and  the  lighter 
coloured  yellow  and  green  region  represent  flat  terrain.  Since  the  terrain  behind  the 
wall  can  not  be  scanned,  the  region  behind  the  wall  is  coloured  white  marking  it  as  an 
unknown  area. 


v  lUpMiro  Ttmlfi  (NeSffl 

gohol 

Pouion  0  0  mm.  Hudng  -777.  CMu  \UtHnq  00 


Figure  16:  Typical  Terrain  Map 

Figure  17  illustrates  the  terrain  map’s  wrapping  capabilities. 

The  initial  map,  built  while  the  Raptor  was  stationary,  has  a  white  region  across  the 
bottom  of  the  map,  which  denotes  the  dead-zone.  An  obstacle8,  denoted  in  red,  is 

8This  obstacle  is  relatively  tall  since  the  nodding  laser  scans  it  in  its  peripheral  vision. 
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Figure  1 7:  Wrapping  Terrain  Map 


located  in  the  botton  right  comer  of  the  map.  The  map  tracks  the  Raptor’s  position  as  it 
moves  and  the  scanned  terrain  migrates  towards  the  bottom  of  the  map.  This  tracking  is 
evident  in  Figure  17  where  the  dead-zone  disappears  and  the  obstacle  moves 
downwards,  out  of  the  mapping  region. 

Figure  1 8  shows  another  illustrative  terrain  map.  In  this  Figure  the  Raptor  vehicle  is 
positioned  directly  in  front  of  a  wall.  The  wall,  denoted  in  red,  almost  completely 
obscures  the  map’s  field  of  view.  Thus,  the  majority  of  the  terrain  map  is  tagged  as 
unknown  and  displayed  in  white. 


Figure  18:  Obstructed  Terrain  Map 

Figure  19  shows  the  Raptor  vehicle  performing  a  left  turn.  The  obstacle,  in  the  bottom 
right  of  the  figure,  shuffles  across  the  map  as  the  Raptor  makes  the  turn.  The  right  side 
of  the  map  reverts  to  unknown  status  when  it  no  longer  has  valid  data.  But  as  the 
nodding  SICK  laser  scans  the  unknown  region,  the  map  is  fused  with  new  3-D  data, 
and  the  terrain  once  again  has  a  valid  representation. 

This  figure  also  reveals  that  the  map's  density  becomes  sparser  as  a  result  of  the  turn. 
As  the  vehicle  rotates  it  quickly  faces  unknown  terrain  and  invalidates  previously 
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Figure  19:  Rotating  Terrain  Map 


scanned  regions.  The  nodding  SICK  laser,  even  in  the  tandem  configuration  where 
there  are  two  forward  looking  lasers,  does  not  have  the  data  rates  that  allow  the  map  to 
maintain  its  density. 

The  terrain  maps,  generated  for  the  ALS  demonstration,  relied  exclusively  on  nodding 
SICK  laser  3-D  data.  Although  the  terrain  map  implementation  handled  3-D  data  from 
the  Digiclops  camera,  this  data  was  not  used.  Early  field  trials  revealed  the  Digiclop’s 
3-D  data  was  especially  prone  to  fringe  effects  where  the  extremities  of  its  field  of  view 
yielded  erroneous  and  unreliable  data. 

As  described  earlier  in  this  report,  terrain  maps  enable  autonomous  manoeuvres. 

Terrain  maps  are  analyzed  for  traversibility  creating  a  Traversibility  Map,  as  shown 
Figure  20. 


Figure  20:  A  Traversibility  Map  overlaid  with  25  Candidate  Arcs,  Obstacles:  Blue,  Traversable:  Ted 


The  traversibility  map  labels  terrain  as  traversable,  obstacles  or  unknown.  The  path 
planner  tries  numerous  candidate  arcs  and  marks  an  arc  as  invalid  if  it  encounters  an 
obstacle,  as  can  be  seen  in  Figure  20. 
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8.  Conclusions 


A  prerequisite  of  autonomous  navigation  is  an  adequate  world  representation.  Using  a 
world  representation,  traversibility  analysis,  and  obstacle  avoidance  behaviours,  a  UxV 
can  safely  navigate  within  its  environment.  This  report  researched  the  current 
state-of-the-art  for  world  representation  techniques  before  concluding  outdoor, 
unstructured  environments  are  best  represented  by  the  terrain  map  technique. 

Many  terrain  map  implementations  rely  upon  single  sample  perception  devices;  thus, 
the  map's  data  fusion  is  relatively  insensitive  to  pose  errors.  3-D  ranging  techniques 
such  as  stereo  vision,  or  ’’stop  and  scan”  techniques  are  also  insensitive  to  pose  errors. 
DRDC’s  continuous  motion  mapping,  using  an  inexpensive  nodding  SICK  laser, 
generates  multiple  terrain  samples;  thus  DRDC’s  terrain  map  implementation  is  pose 
sensitive.  To  minimize  pose  error  effects  DRDC  proposes  the  variance  weighted 
statistical  technique  to  fuse  new  3-D  data  into  the  terrain  map.  A  detailed  analysis  of 
pose  error  sources  [8]  revealed  roll,  pitch  and  yaw  as  the  dominant  error  source.  Using 
the  3-D  data’s  estimated  positional  error,  the  variance  weighted  technique  optimally 
fuses  new  data  into  the  terrain  map. 

Experiments,  conducted  under  static  conditions,  revealed  the  variance  weighted 
technique  performed  slightly  better  than  a  classical  statistics  implementation. 
Simulations,  representing  the  dynamic  conditions  of  a  moving  platform,  disclosed  the 
performance  of  the  variance  weighted  technique  as  superior  to  classical  statistics. 

A  wrappable,  rotating,  variance  weighted  terrain  map  was  implemented  under  the 
MIRO  framework.  The  MIRO  framework  allowed  the  map  to  simultaneously  consume 
3-D  data  from  multiple  data  sources  using  the  vehicle’s  pose  to  maintain  the  map’s 
egocentricity.  A  demonstration,  conducted  in  the  fall  of  2005,  exercised  the  map's 
capabilities.  During  these  trials  DRDC’s  terrain  map  successfully  and  adequately 
represented  the  world.  The  map  remained  egocentric  while  the  Raptor  performed 
translational  and  rotational  manoeuvres.  The  terrain  maps,  once  processed  for 
traversibility,  supported  local  manoeuvres  and  allowed  the  Raptor  to  successfully  avoid 
obstacles  and  hazards.  Hard  turn  manoeuvres  would  often  result  in  a  sparse  map  .  The 
roots  of  this  problem  were  traced  a  lack  of  data,  stemming  from  the  nodding  SICK 
laser’s  slow  sample  rate.  To  help  ameliorate  this  condition,  dual,  forward  looking  lasers 
were  mounted  on  the  Raptor.  The  dual  lasers  helped  increase  the  3-D  data  densities  but 
the  map  fringes  still  suffered  from  sparsity  problems. 

The  terrain  map’s  efficient  implementation  consumed  relatively  small  amounts  of 
computing  resources.  It  could  easily  handle  multiple  nodding  SICK  laser  devices,  as 
well  a  320x240  stereo  vision  frames  twice  per  second.  This  excess  overhead  means  this 
terrain  map  implementation  can  be  expanded  to  meet  future  requirements. 

The  current  terrain  map  implementation  maintained  its  egocentricity  relative  to  the 
Raptor’s  front  bumper.  This  reference  point  often  proved  problematic  since  the  obstacle 
ceased  to  exist  once  the  vehicle’s  front  bumper  passed  by  it.  This  led  to  a  behaviour 
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where  the  Raptor,  believing  the  obstacle  had  been  avoided,  would  turn  sharply  and  side 
swipe  the  obstacle.  Future  terrain  maps  will  move  the  reference  point  to  the  rear 
bumper  and  eliminate  the  root  cause  of  this  problem. 
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